#include "KMotionDef.h"
#include "MySpindleDefs.h"


//  R I G I D   T A P P I N G 

#define ZAXIS 2			//Axis to slave
#define SPINDLE_AXIS 7		//Spindle Axis With Encoder Feedback (Different From SPINDLEAXIS)
#define Z_CPI 34995		//Counts per inch of slave axis
#define CPR 4096		//Counts per rev of spindle axis
#define TAU 0.010		//Smoothing time
#define MAXRPM 2000	        //Maximum Tapping speed in rpm
#define APPROACHRPM 200         //Speed at which the spindle creeps up to target tap depth

double SlaveGain,ToCut,Total,TPI,Z0,S0,ZDir,OSDecel,OSStop;
void Slave(void);

main()
{
	
        float speed = *(float *)&persist.UserData[KMVAR];       //Current spindle speed, set S program to VAR 113
	float TPI   = *(float *)&persist.UserData[0];      	//(MCode Parameter P) Threads Per Inch
        float Depth = *(float *)&persist.UserData[1];           //(MCode Parameter Q) Depth in incremental coords from current position
	float SDir  = *(float *)&persist.UserData[2];      	//(MCode Parameter R) Thread Direction (1=RH, -1=LH)
	
	float DPR = 1/TPI;
	
	printf("DPR=%f\n",DPR);
	printf("Depth=%f\n",Depth);
	printf("Dir=%f\n",SDir);

	// Check to see if commanded speed is less than maximum allowable speed for tapping

	if (speed > MAXRPM){
		speed = MAXRPM;
	}

	// Check if valid R word (rotational direction)

	if (SDir != 1 && SDir != -1)
	{
		printf("Invalid R Word\n");
		printf("Exiting Tapping Function\n");
		return;
	}

	// Stop Spindle

	Jog(SPINDLEAXIS, 0.0);
	while (!CheckDone(SPINDLEAXIS));
	ClearBit(SPINDLECW_BIT);
	ClearBit(SPINDLECCW_BIT);
	printf("Jogging Spindle Stop1\n");
	persist.UserData[STATEVAR] = 0;  // remember we are Off
	while (!CheckDone(SPINDLEAXIS)) ;
        Zero(SPINDLEAXIS);

        {
		WaitNextTimeSlice();
	}


	// Prep Tapping Parameters
	
	//OSStop = 0;
	//OSDecel = 0;
	OSStop = Z_CPI * DPR * (0.0000013939437354374*powf(APPROACHRPM, 2) + 0.0001772126026723660*APPROACHRPM);	// Overshoot Stop
	OSDecel = Z_CPI * DPR * (0.0000013939437354374*powf(speed, 2) + 0.0001772126026723660*speed + 0.75);		// Overshoot Decel

	ZDir = 0; 									// Initial Z Axis Movement Direction ( 0 = Down )	
	int Done = 0;
	int Slowed = 0;


	SlaveGain = Z_CPI * DPR * (-SDir) / CPR;
	Z0 = chan[ZAXIS].Dest;

        Zero(SPINDLE_AXIS);
	S0 = chan[SPINDLE_AXIS].Position;

	float ToCut = Depth * Z_CPI;                   					//Calculted incremental distance
	float DecelPoint = Z0 - ToCut + OSDecel;          					//Position where spindle decelerates to 200 RPM (Absolute Position)
	float StopPoint = Z0 - ToCut +  OSStop;					//Position where spindle is command to stop (Absolute Position)
	float Bottom = Z0 - ToCut;

	
	printf("Decel Point=%f\n",DecelPoint);
	printf("Stop Point=%f\n",StopPoint);
	printf("Bottom=%f\n",Bottom);

	// Start Spindle
	
	WaitNextTimeSlice();

	persist.UserData[STATEVAR] = SDir;  						// Remember spindle direction
	if (SDir == 1)
	{
		SetBit(SPINDLECW_BIT);
	} else {
		SetBit(SPINDLECCW_BIT);
	}
	Jog(SPINDLEAXIS, FACTOR);
        printf("speed",Spindle RPMs for tapping);

	// Begin Tapping Operation
	
        while(Done == 0)        //When Done is equal to 1 then tapping is done
	{
		Slave();
                if ((chan[ZAXIS].Position <= DecelPoint) && (ZDir == 0) && (Slowed == 0))           // Decel Point Reached. Ramp Spindle Speed to Decel Speed
		{
			Jog(SPINDLEAXIS, (5.99924E-16 * powf(decelSpeed, 5) - 1.71704E-11 * powf(decelSpeed, 4) + 1.71893E-07 * powf(decelSpeed, 3) - 0.000842522 * powf(decelSpeed, 2) + 8.157268249 * decelSpeed- 298.3446572));
			Slowed = 1;
		}
		if ((chan[ZAXIS].Position <= StopPoint) && (ZDir == 0))				// Bottom Reached
		{

			if (SDir == 1)
			{
				ClearBit(SPINDLECW_BIT);
				Jog(SPINDLEAXIS, 0.0);
                                while (!(CheckDone(SPINDLEAXIS)))                       // Wait until SPINDLEAXIS is done moving and Spindle Zero Speed Relay goes high
				{
					Slave();					// Continue moving Z-Axis while spindle slows to a stop
				}
	
                SetBit(SPINDLECCW_BIT);                                 //Reverse Direction
				Jog(SPINDLEAXIS, FACTOR);
				persist.UserData[STATEVAR] = -1;  			// remember spindle is set to CCW

			} else if (SDir == -1){
				
				ClearBit(SPINDLECCW_BIT);
				Jog(SPINDLEAXIS, 0.0);
                                while (!(CheckDone(SPINDLEAXIS)))
				{
					Slave();					// Continue moving Z-Axis while spindle slows to a stop
				}	
                SetBit(SPINDLECW_BIT);                                  //Reverse Direction
				Jog(SPINDLEAXIS, FACTOR);
				persist.UserData[STATEVAR] = 1;  			// remember spindle is set to CW
			
			}

			ZDir = 1;
			
		}

		if ((chan[ZAXIS].Position >= Z0) && (ZDir == 1))				// Tap Complete
		{
		
			Done = 1; 
	// spin down
	Jog(SPINDLEAXIS,0.0);
	while (!CheckDone(SPINDLEAXIS));
	ClearBit(SPINDLECW_BIT);
	ClearBit(SPINDLECCW_BIT);
	printf("Jogging Spindle Stop2\n");
	persist.UserData[STATEVAR] = 0;  // remember we are Off
	while (!CheckDone(SPINDLEAXIS)) ;


		}	

	}

	Move(ZAXIS, Z0); 								// Move ZAxis to initial position
	while (!CheckDone(ZAXIS));
	printf("Tap Function Complete\n");

	    
}

void Slave(void)
{
	float Destination = (chan[SPINDLE_AXIS].Position-S0) * SlaveGain + Z0;
	MoveExp(ZAXIS, Destination, TAU);
	WaitNextTimeSlice();
}